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ABSTRACT 

This  paper  presents  a  robotic  assembly  of  2  3-D  jig-saw  puzzle 
pieces,  using  vision,  position  and  force  feedback.  Geometrical  aspects  are 
discussed  together  with  force  history  of  the  experiments.  An  installation 
using  7565  I.B.M.  robot  with  a  CCD  camera  on  the  robot  arm  is 
described.  Successful  experiments  proved  the  possibility  of  assembling 
parts  with  complex  contours  when  force  feedback  is  used  during  fine 
assembly.   A  videotape  of  the  demonstration  is  also  available. 
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1.   Introduction  - 

Automated  assembly  of  variable  and  complex  shaped  parts  with  rela- 
tively small  tolerances  is  a  substantially  more  demanding  task  than  the  peg  in 
a  hole  problem  treated  in  literature.  Inoue  [6]  Simmunovic[10]  Seltzer[9] 
have  discussed  the  force  aspects  of  peg  in  a  hole  insertion,  and  demonstrated 
successful  mating  with  small  tolerances.  Their  approach  was  to  consider  the 
geometry  of  parts  as  known,  and  use  complex  force  sensors  (such  as  the 
Instrumented  Center  of  Compliance  )  to  accommodate  positioning  errors  dur- 
ing assembly.  Further  work  by  Lozano-Perez[l]  ,[8]  demonstrated  the 
automated  synthesis  of  fine  motion  strategies  for  circular  peg  in  hole  inser- 
tion. Buckley  [3]  extends  the  automated  synthesis  to  rectangular  pegs  in  rec- 
tangular holes.  The  experimental  part  of  his  thesis  uses  a  similar  I.B.M. 
7565  robot  as  the  one  used  by  us,  while  the  tolerances  were  also  of  the  same 
order  of  magnitude  (0.03  inches).  Buckley's  experiments  were  more  general 
than  the  circular  peg  approach,  since  no  axial  symmetry  was  present.  How- 
ever in  his  motion  synthesis  Buckley  simplifies  the  general  problem  in  that  he 
does  not  address  rotational  errors  during  assembly.  This  errors  do  usually 
develop  and  are  crucial  for  parts  with  asymmetrical  contours  such  as  rec- 
tangular pegs. 

The  present  work  addresses  aspects  related  to  robot  assembly  of  variable 
shaped  parts  with  no  axial  symmetry  such  as  jig-saw  puzzles.  Due  to  their 
complex  geometry,  rotational  errors  cannot  be  overlooked  and  are  addressed 
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during  the  fine  assembly  part  of  our  experiment.  We  chose  to  use  puzzle 
pieces  since  Kalvin,  Schonberg  and  Wolf  son  [7]  ,[12]  developed  algorithms 
for  simulated  jig-saw  puzzle  assembly  using  vision  data.  They  chose  to  use 
only  the  contour  of  the  puzzle  pieces  rather  than  the  actual  picture  presented 
by  the  puzzle,  which  led  to  the  idea  of  attempting  a  robotic  verification  of 
Wolf  son's  algorithm,  which  sometimes  may  give  erroneous  solutions[4] 

This  paper  describes  techniques  developed  for  automatic  assembly  of  two 
puzzle  pieces.  Future  work  will  extend  this  results  to  robotic  assembly  of  a 
whole  jig-saw  puzzle.   The  outline  of  this  report  is  as  follows: 

Qiapter  2  describes  the  experimental  installation,  sensor  calibration  issues 
as  well  as  puzzle  piece  modification  for  robot  assembly. 
Qiapter  3  presents  vision  to  robot  transformations  used  for  part  detection 
and  pick  up. 

Chapter  4  develops  the  geometrical  transformations  used  during  robot 
assembly.  Robot  moves  are  structured  in  raw  positioning,  transition  to 
fine  assembly  and  fine  assembly  moves. 

Chapter  5  presents  force  readings  during  successful  experiments  as  they 
relate  to  the  type  of  robot  moves  analyzed  in  Chapter  4. 
Chapter  6  determines  acceptable  level  of  errors  for  successful  assembly. 
Here  both  geometric  and  sensorial  implications  are  addressed  and  theoret- 
ical predictions  are  compared  with  experimental  results. 
The  Appendix  contains  some  of  the  software  developed  for  vision  and 
force  assembly. 


2.   Experimental  Installation 


2.1.  Description 

The  experimental  system  as  presented  in  Fig.  1,  2  and  3,  consists  of  an 
I.B.M.  7565  robot,  a  vision  system,  force  sensors  and  mechanical  force 
amplifier.  The  vision  system  uses  an  existing  Fairchild  CCD  camera,  with  a 
256x256  sensor  array  and  a  VICOM  image  processor  with  512X512  array. 
The  Fairchild  camera  sensor  head  is  installed  directly  on  the  robot  arm,  at  a 
fixed  height  above  the  robot  work  table.  This  eliminates  the  need  for  special 
lense  drives  to  compensate  for  variable  focal  lengths,  while  allowing  a  com- 
plete scan  of  the  table.  The  robot  work  area  was  covered  with  white  photo- 
graphic paper,  and  the  wooden  puzzles  were  covered  with  black  nonreflective 
paper  in  order  to  boost  the  vision  image  quality. 

A  VAX  minicomputer  supervises  the  vision  process  and  communicates 
with  a  SERIES  1  minicomputer  which  is  the  robot  controller.  Force  sensors 
on  the  robot  gripper  allow  for  force  feedback  during  assembly.  In  an  earlier 
design  [5]  we  included  a  special  vacuum  tool  for  parts  handling.  This  was  not 
finally  implemented  due  to  unstable  grasp,  slip  and  potential  piece  loss  when 
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subject  to  assembly  torques.  This  tool  was  replaced  with  a  combination  of 
handle  on  the  piece  and  specially  designed  mechanical  force  amplifier  as 
presented  in  Fig.  3.  The  criteria  that  led  to  the  final  gripper  nails  design 
were: 

(1)  stable  four  point  grip  on  cylindrical  handle; 

(2)  adaptive  grip  during  initial  part  pickup  (allowing  the  use  of  center 
grasping); 

(3)  no  interference  with  existing  part  presence  sensor  on  the  gripper; 

(4)  mechanical  force  amplification. 
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Figure  1   Experimental  installation  for  puzzle  assembly 
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Fig.  2  IBM  7565  robot  with  vision  system  (above) 
Fig.  3  CCD  camera  on  robot  arm  (detail) 
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Fig.  4  Special  gripper  nails  for  7565  I.B.M.  robot 
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2.2.   Piece  preparation 

For  our  experiment  we  selected  an  of  the  shelf  wooden  puzzle  presented 
in  Fig.  5.  The  vision  matching  algorithm  uses  only  the  piece  shape, 
therefore  we  covered  the  piece  picture  with  black  nonreflective  paper. 
This  was  necessary  in  order  to  boost  the  quality  of  vision  images.  It  is 
important  that  the  pieces  do  not  slip  on  the  work  table  during  robot 
assembly.  Therefore  the  bottom  of  the  puzzle  pieces  was  fitted  with 
Koroseal  flexible  magnetic  strip[l\  in  friction  that  prevents  sliding  at 
small  assembly  forces.  The  magnitude  of  the  magnetic  force  determines 
the  force  envelope  that  the  robot  uses  during  assembly.  This  forces  were 
determined  experimentally  to  be  about  1.4  N/sq.  inch  or  about  3  N  for 
puzzle  pieces  with  a  surface  of  2  to  3  sq.  inches.  The  robot  gripper  con- 
figuration (2  parallel  fingers)  does  not  allow  direct  grasping  of  the  puz- 
zle pieces.  It  was  therefore  necessary  to  mount  a  cylindrical  handle  on 
one  of  the  puzzle  pieces  corresponding  to  its  center  of  gravity.  In  this 
way  the  robot  grasps  on  the  handle  when  picking  up  the  puzzle  piece. 
Grasping  has  to  be  done  without  pushing  or  overturning  the  puzzle 
piece.    The  maximum  grasping  force  used  in  center  grasp  is  given  by 


(2.1). 


^  grasp  ,  \^-^) 


^ grasp  ^  maximum  grasping  force  used  for  center  grasping 
M  =  total  magnetic  pull 

/  =  distance  from  the  center  of  gravity  to  the  puzzle  edge 
h  =  grasping  force  application  point  versus  work  table 

M^k„,,A  (2.2) 

^mag  —  Specific  magnetic  pulling  force 
A  =  puzzle  piece  area 

The  grasping  force  application  point  h  depends  on  the  handle  length  as  well 
as  the  arm  vertical  coordinate  and  the  gripper  length  as  expressed  in  (2.3). 

,  •"    '  ^      ^table       ^gripper  /^   t\ 


H  -  handle  height  above  work  table 

Z  =  robot  arm  Z  coordinate 

Ziabie  =  Z  coordinate  of  work  table  surface 


hripp<=r  =  robot  gripper  length 
With  (2.2)  and  (2.3)  equation  (2.1)  becomes  (2.4). 


^'^mag^'' 


grasp 


gripper       ^table 


(2.4) 


Using  the  physical  parameters  of  the  materials  used,  equation  (2.4)  becomes 
(2.5) 

'^^'°'P  "  0.213  +  0. 066Z  ^^-^^ 

which  yields  grasping  forces  of  about  1.2  N,  without  pushing  the  piece. 


Fig.  5  Wooden  jig-saw  puzzle  pieces  used  for  assembly 
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2.3.   Force  sensor  recalibration 

In  section  2.2  we  determined  that  horizontal  assembly  forces  should  not 
exceed  3  N  in  order  to  avoid  pushing  the  puzzle  pieces  over  robot  work  table. 
This  condition  is  even  more  restrictive  for  piece  pick  up  when  the  grasping 
force  should  be  less  than  2  N.  The  7565  I.B.M.  robot  gripper  is  equipped 
with  force  sensors  that  provide  3D  force  information.  This  sensors  are  fac- 
tory designed  for  a  range  of  0  to  100  N,  with  low  resolution  in  the  range  of 
forces  used  during  puzzle  assembly.  We  have  therefore  taken  several  steps  to 
amplify  signal  in  the  low  senor  range.  One  step  is  to  modify  the  sensor  cali- 
bration lookup  table  thus  obtaining  100%  amplification  for  small  forces.  A 
special  table  was  created  during  a  one  time  off-line  calibration,  and  the  low 
level  software  loaded  in  the  system  library.  Since  no  high  level  software 
arithmetics  is  involved,  there  was  no  negative  impact  on  the  overall  run  time 
of  the  application.  Fig.  6  presents  sensor  readings  before  and  after  recalibra- 
tion, showing  a  100%  amplification  for  small  forces.  The  adverse  result  of 
this  procedure  was  an  increase  in  signal  noise  (or  uncertainty)  from  0.15  to 
0.30  N.  The  next  step  to  increase  readings  was  to  add  specially  designed 
nails  to  the  gripper.  These  nails,  which  are  presented  in  Fig.  4,  work  as 
mechanical  force  amplifiers  producing  50%  pitch  and  side  force  amplification. 


3.   Vision  steps  for  assembly 

The  vision  steps  required  by  piece  detection,  feature  extraction  and 
visual  match  are  schematically  presented  in  Fig.  7.  The  convention  was  to 
name  the  stationary  puzzle  piece  as  Piece  1  while  the  piece  that  is  picked  up 
by  the  robot  arm  was  named  Piece  2. 

3.1.   Camera  to  robot  coordinate  transformations 

For  both  visual  search  and  piece  pickup  it  is  necessary  to  establish  the 
correspondence  between  image  and  robot  spaces.  This  relation  is  determined 
by  a  number  of  parameters  such  as  camera  position  offset  versus  the  robot 
arm,  camera  x/y  distortion  factor  as  well  as  camera  projection  factor.  We 
define  the  camera  position  offset  as  the  distance  from  the  image  center  to  the 
x/y  robot  link  in  robot  coordinates.  Here  z  coordinate  is  fixed  and  no  rota- 
tion offset  is  taken  into  account.  The  x/y  distortion  factor  is  specific  to  each 
CCD  camera  and  represents  the  number  of  pixels  that  corresponds  to  a  given 
length  along  x  and  y  axis.  The  projection  factor  depends  on  the  lens  used  as 
well  as  camera  to  work  table  distance.  It  represents  the  number  of  inches 
that  correspond  to  one  pixel  in  the  image.  Taking  all  above  factors  into 
account,  camera  to  robot  coordinate  transformations  are  expressed  by  (3.1) 
and  (3.2). 
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Set  up  calibration  picture 

Extract  boundary  corners 
(Piecel.sum.out  file) 

Scan  robot  work  table 
for  Piece  1  and  Piece  2 

Extract  boundary  corners 
(Piece2.sum.out  file) 

Extract  boundary  shape 
(Piecel.sum  file) 

Piece  (1  or  2)  descovered 

Extract  boundary  shape 
(Piece2.sum  file) 

Extract  robot  coordinates 
of  Piece  1 

Center  on  piece 

Extract  robot  coordinates 
of  Piece  2 

>' 

Match  Piece  1  and  Piece  2 

Calculate  rotation  and  transl. 
for  matching  sides 

Calculate  rotation  and  transl 
for  Piece  2 

Assembly  steps  i 


Fig.  7  Vision  steps  for  assembly 


X,,i,,,=  {256-X,i,,^)*PROJECTION-X,ff,,,+X  (3.1) 

{Y,i,,„  -  240)*  PROJECTION 
XSCALE 


^  robot  ~  Y^r'ATrr  ^  offset^  ^  W--^^ 


Xyicom .  ^vicom  =  point  VICOM  coordinates 
^robot ,  ^ robot  =  poiiit  lobot  coordinatcs 
PROJECTION  =  projection  factor 
Xoffset  —  camera  offset  in  robot  x  direction 
^£#5^;  =  camera  offset  in  robot  y  direction 
XSCALE=  camera  distortion  factor 
X,Y  =  robot  arm  coordinates 

3.2.  Work  table  visual  scan 

As  a  matter,0f  generality  the  puzzle  pieces  are  placed  randomly  on  the 
work  table,  and  their  location  is  not  known  to  the  assembly  algorithm.  There 
are  however  limitations  on  the  puzzle  piece  position  which  has  to  be  within 
the  intersection  of  camera  field  of  view  and  robot  work  envelope.  Let  us 
define  XrobminYrobmir,XrobmaxYrobmax  ^s  the  minimum  and  maximum  robot 
X,Y  coordinates.  Similarly  X^amminJcamminXcammaxycammasc  are  the  camera 
field  of  view  minim  and  maxim  X,Y  coordinates.  Combining  vision  and 
robot  assembly  means  that  for  a  Cartesian  robot  such  as  7565,  puzzle  piece 
coordinates  have  to  satisfy  (3.3)  and  (3.4). 

■^puzzle       \/^cammin,-^cammax)  (l  \A-robmin,^robmax )  V         / 

^puzzle       \^  cammin,^  cammax)  11  \^  robmin,^  robmax )  \-^-^) 

The  restrictions  (3.3),  (3.4)  limit  the  space  that  is  scanned  by  the 
camera/robot  assembly  in  order  to  discover  the  puzzle  pieces.  This  scanning 
space  is  subdivided  in  quadrants  which  overlap  so  that  the  low  quality  border 
side  of  images  can  be  discarded.  During  this  initial  assembly  stage  ,  we  are 
interested  to  determine  whether  the  puzzle  pieces  exist  rather  than  extracting 
their  shape.  The  visual  scan  therefore  uses  compressed  images  that  give  a 
significant  speedup  of  processing.  Each  quadrant  image  is  subsequently 
analyzed  to  detect  part  presence.  If  a  part  exists,  ANALYZE  returns  its  raw 
center  of  gravity  in  vision  coordinates.  The  vision  system  is  a  2D  system  that 
takes  images  of  a  3D  scene.  This  produces  vision  errors  if  pieces  are  not  cen- 
tered under  the  camera.  The  robot  has  therefore  the  additional  task  of 
centering  the  camera  on  top  of  a  discovered  piece  in  order  to  minimize  vision 
errors.   This  is  done  by  the  routine  CENTER. 
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Fig.  8  CORNER  output 


3.3.   Shape  extraction  and  visual  match 

Once  the  camera  is  centered  on  the  puzzle  piece,  its  shape  is  extracted 
using  SHAPE  and  CORNER  routines  (Edith  Schonberg).  The  output  is  a 
sampled  curve  divided  into  four  sides  as  presented  in  Fig.  8.  This  becomes 
input  for  MATCH  routine  (A.  Kalvin)  that  returns  a  score  for  matching  sides 
of  two  puzzle  pieces.  Our  algorithm  selects  the  smallest  of  16  match  scores, 
corresponding  to  the  2  sides  that  most  likely  match.  MATCH  also  returns 
the  rotation  and  translation  of  the  matching  sides.  An  automated  rotation 
and  translation  is  done  using  the  handle  on  the  puzzle  piece,  rather  than  the 
matching  sides.  The  target  for  center  of  gravity  of  the  rotated  puzzle  piece  is 
obtained  from  MATCH  output  using  (3.5),  (3.6). 
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Pn^+LEFTXiXSCALE-l) 

cosmo^-Pu) 

XSCALE  ^      ' 

G3y=P2y  + XSCALE +cos{Q){G^y-Piy)  (3.6) 

G-ijcy  =  center  of  gravity  of  rotating  piece  after  rotation 
G  Lc,y  =  center  of  gravity  of  rotating  piece 
Pu,y  —  center  of  gravity  of  matching  side  on  rotating  piece 
P2x,y  —  center  of  gravity  of  matching  side  on  stationary  piece 
0=  rotation  angle  of  matching  side  (given  by  MATCH) 
LEFTX  =  leftmost  X^icom  coordinate  of  stationary  piece 
XSCALE  =  camera  distortion  factor 

4.   Robot  assembly  steps 

The  robot  assembly  moves  are  presented  schematically  in  Fig.  9.  We 
have  adopted  Simmunovic's[10]  notation  when  considering  the  over  all 
automated  assembly  as  divided  in  three  steps:  raw  positioning,  transition  to 
fine  assembly  and  fine  assembly.  During  raw  positioning  Piece  2  is  picked 
up,  rotated  and  translated  to  Piece  2,  tilted  and  lowered  on  top  of  Piece  1. 
The  raw  positioning  task  is  to  bring  Piece  2  in  contact  with  Piece  1  in  such  a 
way  as  to  allow  further  assembly  moves  to  eliminate  both  rotation  and  trans- 
lation errors.  This  leads  to  a  purposely  induced  overshoot  of  Piece  2  so  that 
the  relative  position  of  the  two  pieces  is  known.  During  transition  moves  the 
two  pieces  remain  in  contact  while  their  relative  position  changes  to  a  nested 
configuration.  It  is  during  transition  moves  that  translational  errors  are  eUm- 
inated.  The  final  and  crucial  step  is  fine  assembly  based  on  force  feedback. 
During  these  moves  Piece  2  is  pushed  into  Piece  1  while  rotational  errors  are 
eliminated.  Finally  Piece  2  is  tilted  back  to  horizontal  configuration  without 
contact  loss  with  Piece  1.  This  represents  the  last  step  in  fine  assembly.  The 
following  subsections  give  more  details  regarding  the  assembly  moves,  while 
Chapter  5  depicts  force  sensor  history  for  actual  assembly s. 
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Vision  steps 


I 


If  matching  pieces 
Pick  up  Piece  2 


Rotate  Piece  2  over  CG 
Translate  Piece  2  to  Piece  1 


Tilt  Piece  2  with  CG  of 
matching  side  fixed 


Lower  Piece  2  over  Piece  1 
until  contact 


Move  to  edge  1  and  2  and 
center 


Move  to  edge  3  and  4  and 
center 


Push  in  Piece  2  into  1 
(Piece  2  still  tilted) 


Tilt  back  Piece2 
Release  Piece  2 


Raw  positioning  based  on 
^     vision  data 


< 


< 


Transition  from  raw  pos'  itioning  to 

fine  motion  assembly 

(here  eliminate  translation  errors) 


Fine  motion  with  force  and 
position  controll 
(here  rotate  Piece  2  to  eliminate 
rotation  errors) 


Fig.  9  Robot  assembly  steps 


IS 


4.1.  Raw  positioning  based  on  vision  data 

4.1.1.  Approaching  move  in  free  space 

The  robot  goal  for  initial  approach  of  Piece  1  after  picking  up  Piece  2  is 
given  by  (4.1)  and  (4.2). 

G^,robot  =  {'2.56-G^,)PROJECTION-X,ff,,,+X^  (4.1) 

{G,y-2AQ)PR0JECTI0N 
tj  2,y  robot  —  XSCALE  °^^"'  y^-'^J 

G3:crobot,3y robot  =  Tobot  target  for  approach 

G3;ty  =  center  of  gravity  of  moving  piece  after  rotation  given  by 

(3.5-6) 

Xi,Yi   —  robot  arm  coordinates  when  stationary  piece  was  in 

center  of  image 

The  Z  arm  coordinates  during  this  move  are  determined  by  collision 
avoidance  criteria.  Since  no  obstacles  are  present,  this  becomes  a  free  space 
move.  APPROACH  calculates  X,Y  coordinates  for  this  move  and  is 
included  in  Appendix. 

4.1.2.  Tilting  move 

The  piece  that  is  picked  up  by  robot  has  a  convexity  that  has  to  get  into 
the  concavity  of  the  stationary  piece.  Generaly  speaking,  the  convexity 
becomes  an  asymmetric  peg  and  the  concavity  an  asymmetric  hole.  The  meas- 
ured average  tolerance  for  assembly  (0.03  inches)  is  the  same  order  of  magni- 
tude as  camera  projection  factor.  It  is  therefore  necessary  to  tilt  the  piece  in 
order  to  encrease  tolerances.  After  tilting  tolerances  are  about  the  same  as 
the  concavity  diameter.  The  geometry  of  the  two  puzzle  pieces  determine  the 
actual  tilting  angle.  For  our  case  this  angle  is  10  degrees.  Let  us  define  a 
Cartesian  Object  System  of  Coordinates  as  presented  in  Fig.  10.  The  origin  of 
this  system  of  coordinates  is  the  center  of  gravity  of  the  side  with  convexity 
(peg)  Pi,  while  Y^hj  passes  through  Gi.  During  tilting  Pi  is  maintained  at 
fixed  X,Y,Z  robot  coordinates.  Object  to  robot  coordinate  transformations 
are  given  by  equations  (4.3),  (4.4)  and  (4.5). 

X^X,,hot  +  sini^)ia-Y,hj)  +  cosi^)X,i,j  (4.3) 

Y=Y,,,,,  +  5m{^)X,i,j  +  cos{^)iY,,j-a)  (4.4) 

Z=Z,,,,,+Z,,j-f-b  (4.5) 
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X,Y ,Z  —  point  in  robot  system  of  coordinates 

■^roiof  >  ^roZ>oM-^roior  —  robot  arm  coordinates   in  robot  system  of 

coordinates 

Xoi,j,Yobj,Zobj=  point  coordinates  in  object  system  of  coordinates 

P  =  rotation  angle  between  robot  and  object  coordinate  systems 

a  —  distance  from  P  i  to  G  i 

b  =  handle  length 

/  =  distance  from  handle  top  to  robot  arm  X,Y,Z  link 

6=  tilting  angle 

Robot  target  for  tilting  the  piece  thus  becomes  (4.6)  to  (4.9). 


X:iu ^Xrobot  +  sin  ((B) (a - acos  {Q)+{b  +f)sm  (9)) 

ytiit  =  Yrobot  -  cos  0)(a  -  acos  {Q)  +  {b  +f)sin{Q)y 

Zau=Zrobot+{b^f){cos{Q)-l)  +  asin(Q) 

Pitch  =  e 


(4.6) 
(4.7) 
(4.8) 
(4.9) 


Z  objec 


Zrobot 


^robot 


X  robot 


X  object 


Fig.  10  Object  system  of  coordinates 
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4.1.3.  Bringing  the  pieces  in  contact 

This  is  the  last  move  during  raw  positioning  assembly.  Here  the  free 
space  assumption  does  not  apply,  since  the  move  terminates  upon  contact 
with  Piece  1.  This  is  a  different  termination  condition  than  for  the  previous 
moves,  and  requires  force  feedback.  Will  and  Grossman[ll]  call  this  type  of 
moves  guarded  moves.  Buckley [3]  gives  the  relation  between  position  errors 
and  force  feedback  in  what  he  names  the  generalized  spring  equation  (4.10). 


x=x, 


4 


(4.10) 


X  —  robot  actual  position 
Xc  =  robot  commanded  position 
fr  =  sensed  reaction  force 
k  =  system  stiffness  constant 


Equation  (4.10)  may  ailso  be  written  as  (4.11)  that  gives  reaction  forces  as  a 
function  of  position  errors  that  occur  when  the  robot  arm  moves  against  an 
obstacle  (in  our  case  the  stationary  piece) . 


fr  = 


(4.11) 


Equation  (4.11)  is  the  termination  condition  for  all  transition  and  fine  assem- 
bly moves  described  in  subsequent  chapters. 


Stationary  piece 


PiGi 


Fig.  11  Transition  moves 
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4.2.  Transition  moves 

These  moves  are  done  in  order  to  nest  the  convexity  on  Piece  2  into  the 
concavity  of  Piece  1.  The  first  transition  move  is  a  sliding  on  top  of  Piece  1 
that  terminates  when  contact  is  lost  at  edge  1,  as  presented  in  Fig.  11.  The 
sliding  is  done  along  the  generalized  direction  Pi  Gi.  After  edge  1  was 
detected,  the  next  move  is  towards  edge  2,  where  contact  between  the  two 
pieces  is  reestablished.  An  average  of  the  two  termination  points  represents 
the  target  for  next  move.  The  same  procedure  is  repeated  for  edge  3  and  4, 
but  the  move  is  done  on  a  trajectory  perpendicular  to  PiGi.  Averaging  the 
four  contact  points  eliminates  translation  errors  that  were  induced  by  the 
vision  system. 

4.3.  Fine  assembly  moves 

After  completion  of  transition  moves  Piece  2  is  nested  with  Piece  1,  but 
on  top  of  it  and  tilted .vn5imng"f me  assembly  Piece  2  is  to  be  pushed  down 
into  Piece  1  and  then  tilted  back  to  a  horizontal  position.  The  task  for  tilt 
back  is  to  maintain  contact  with  Piece  1  and  rotate  vertically  around  the  con- 
tact point.  It  is  therefore  necessary  to  calculate  the  distance  JG  i  as  shown  in 
Fig.  13.   Let  /  be  the  length  of  7G 1 . 

^_Z,,,u-(f+b-ht)cosiQ)-t 

I  =  distance  from  the  contact  point  to  Piece  2  center  of  gravity 
t=  piece  thickness 
6  =  tilting  angle 

4.3.1.   Push  in  move  with  rotation  errors 

First  the  convexity  of  Piece  2  is  brought  in  contact  with  Piece  1  by  mov- 
ing Piece  2  back  to  edge  1  along  GiPi.  Once  the  contact  established,  the 
next  move  is  a  pushing  of  Piece  2  into  Piece  1  with  the  target  given  by  (4.13) 
to  (4.16). 

Xpushin  =^Xrobot "  tsin  {^)sin  (9)  (4. 13) 

Yp,,Hin  =  Yrobo,-^tcos{^)sin{Q)  (4.14) 

Zpushin  ^^robot  "  tCOS  (6)  (4. 15) 

Pitched  ,  (4.16) 

The  tilting  angle  9  is  such  that  if  rotation  errors  exist,  the  elbow  of  Piece  2 
hits  the  top  of  Piece  1  before  the  pushing  in  completes.  The  algorithm  has  to 
distinguish  between  the  two  possible  cases  of  right  or  left  elbow  hit.  The 
force- tor  que  system  that  corresponds  to  these  cases  is  presented  if  Fig.  14. 
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Fig.  12  Relative  possition  after  transition  moves 


In  order  to  eliminate  rotation  errors,  Piece  2  is  rotated  by  an  angle  7,  and  the 
goal  for  this  move  is  (4.17)  to  (4.20). 


Xrotalion=Xrobot+  iiC0s{Q)-  (f  +  b)sin  (0)) 

{sin{y  +  ^)  +  sin{^)) 

yroiaiion  =  Y robot  +  {IcOS  {^)-{f  +  b)sin  (9)) 

(cc»5(7+(3)-co^O)) 

^rotation       ^robot 
J  aw  rotation  ^  J  a^  robot  +  7 

X,Y,Z,Jaw,.gtation  —  target  for  rotation  move 
7  —  rotation  angle  around  contact  point  J 


(4.17) 

(4.18) 
(4.19) 
(4.20) 
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Fig.  13  Notation  for  JGl  calculation 


I  obja 


Fig.  14  Force-torque  system  for  elbow  hit 
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The  rotation  move  terminates  when  contact  between  the  two  pieces  is  lost 
which  results  in  a  modification  of  force  readings. 

4.3.2.   Tilt  back 

This  is  the  last  move  of  fine  assembly  which  brings  Piece  2  to  a  horizon- 
tal position  without  loss  of  Piece  1.  During  this  move  the  target  is  given  by 
(4.21)  to  (4.24). 

X^iUbcck  =Xrobot  +  sin  (p  +  7)  (/  -  Icos  (9)  -  (/  +  b)sin  (0))  (4.21) 

Y,atback-Yrobo-cos{^-r'^){l-lcos{Q)-{J  +  b)sinm)  (4.22) 

Z:iuback=Zrobot  +  if'rb){cos  (9)-  l)-/5m  (9)  (2.23) 

:-v.,v.-^vu         Pitchaitback=PitcK^bot-^  (4.24) 

Fig.  15  presents  the  pieces  position  during  tilt  back  move. 


Fig.  15  Tilt  back  move 
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5.   Force  history 

IBM  7565  force  sensor  geometry  is  presented  in  Fig.  16.  Each  gripper 
finger  has  tip,  side  and  pitch  strain  gages  giving  force  readings  in  three  direc- 
tions. Sensor  readings  are  influenced  by  a  number  of  factors  such  as  orienta- 
tion, sensor  offset,  calibration,  mechanical  amplification,  crosstalk,  vibrations. 

A  simple  test  was  conducted  in  order  to  determine  the  effect  of  gripper 
orientation  on  force  sensor  output.  The  gripper  executed  a  full  rotation  from 
-135  to  135  degrees,  while  maintaining  a  vertical  position  (pitch=0,  yaw  =  0). 
During  gripper  rotation  a  constant  5  N  horizontal  force  was  applied,  using  a 
combination  of  handle,  ball  bearings  and  calibrated  weights.  Force  readings 
for  this  experiment  are  presented  in  Fig.  17.  The  results  show  a  clear  force 
reading  dependency  on  gripper  orientation.  For  -45  degrees  rotation  ,  both 
pitch  and  tip  forces  are  close  to  0,  while  side  forces  have  a  maximum.  This  is 
the  orientation  for  which  the  applied  force  was  axially  perpendicular  to  the 
gripper  side.  The  results  of  the  previous  experiment  indicate  that  force  read- 
ing during  assembly  might  depend  on  gripper  orientation  during  part  pick  up. 
It  is  therefore  important  to  determine  what  is  the  best  gripper  grasping  posi- 
tion in  order  to  maximize  sensor  output.  We  consider  this  to  be  a  perpendic- 
ular to  the  general  direction  PiGi,  since  this  is  the  direction  in  which  initial 
contact  forces  appear. 


r<r; 


Fig.  16  IBM  7565  Force  Sensor  Geometry 
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Tip  Force  ws  Gripper  Rotation  ftngle 


Side  Force  ua  Gripper  Rotation  Angle 


Pitch  Force  ws  Gripper  Rotation  Angle 


-156      -leo 


Fig.  17  Force  readings  vs  gripper  orientation 
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Force  readings  during  assembly  are  presented  in  Fig.  18  to  26.  This  readings 
represent  processed  data  that  has  been  compensated  for  sensor  offset  and 
calibration  amplification.  Measured  force  envelope  during  piece  nesting  is  of 
the  order  of  5  N,  which  is  slightly  larger  than  anticipated.  This  might  be  due 
to  the  mechanical  amplification  effect  of  the  handle-gripper  combination. 
Tests  showed  a  mechanical  amplification  of  about  50%  which  means  true  con- 
tact forces  of  about  2.5  N.  This  force  level  meets  conditions  set  in  Chapter 
2,  which  explains  why  no  slip  is  present  during  initial  assembly  steps. 

Force  readings  are  further  influenced  by  cross  coupling  effects  due  to 
sensor  design.  Tests  have  shown  that  cross  coupling  accounts  for  about  5  to 
10%  residual  force  readings,  as  presented  in  Fig.  7.  Second  order  dynamic 
effects  have  not  been  considered  here  since  assembly  is  done  at  very  low 
speed  (0.2  inch/second). 

Fig.  18  to  20  present  force  data  for  nesting  only,  with  no  rotation  error. 
Fig.  21  to  23  are  test  data  for  puzzle  piece  nesting  with  4.63  degrees  rotation 
error.  Finally  Fig.  24  to  26  show  forces  during  a  complete  assembly  includ- 
ing fine  motion  moves.  Here  nesting  is  only  the  initial  part  of  the  graphs, 
where  force  patterns  detailed  in  Fig.  18  to  20  are  recognizable.  EXzring  fine 
assembly  forces  occasionally  pass  the  limit  of  magnetic  pull  resistance,  and 
slip  is  present. 

When  analyzing  Fig.  18  we  note  a  large  tip  force  encrease  from  A  to  B, 
followed  by  a  sharp  decrease  C  to  D.  These  represent  the  end  of  raw  posi- 
tioning moves  at  point  B  and  the  detection  of  edge  1  at  point  D,  as  described 
in  Chapter  4.  Both  sensor  outputs  follow  the  same  pattern  ABCDE,  but 
separate  along  EFG  and  EHI  respectively.  This  corresponds  to  nesting  moves 
that  detect  edge  3  and  4.  Here  both  sensors  are  subject  to  torques  in  oppos- 
ing direction  which  explains  the  mirror  symmetry  of  EFG  versus  EHI.  The 
same  general  pattern  is  followed  by  side  and  pitch  forces.  Fig.  20  details  the 
same  nesting  moves,  but  here  force  variation  corresponding  to  detection  of 
edge  1,  2,  3  and  4  is  less  distinct  compared  to  Fig.  18.  These  results  reflect 
the  fact  that  during  the  test,  initial  contact  was  already  on  edge  4,  and  the 
assembly  algorithm  did  not  succeed  in  nesting  the  2  puzzle  pieces.  It  is  there- 
fore important  to  estimate  the  limit  on  rotational  and  translational  errors  that 
the  assembly  algorithm  can  tolerate. 

6.  Assembly  robustness 

The  vision  solution  given  as  input  to  the  robot  assembly  algorithm  may 
contain  both  rotation  and  translation  errors.  We  define  the  vision  rotation 
error  B^^r  as  given  by  (6.1). 

8,,,  =  8'-0  (6.1) 
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0grr  —  rotation  error  from  vision  solution 
9 '  =  solution  returner  by  vision  match 
8  =  correct  solution 

Due  to  puzzle  asymmetry,  any  rotation  errors  produce  translation  errors  of 
the  initial  contact  point  at  the  end  of  raw  positioning.  This  translation  errors 
are  given  by  (6.2),  (6.3). 

Xj>-Xj^G^^robot'-G2xrobot  +  ^{T  +  l)sin{-^^)cos{ Y^^^       ^^'^^ 

ft                 2R  +  ft 
Yj'-Yj  =  G3yrobot'-G3yrobot  +  '^iT  +  l)sin  {-^)sin  ( ^-^^)       (6-3) 


X,Yj'  =  robot  coordinates  of  contact  point  with  vision  rotation 

error 

X,  Yj  —  robot  coordinates  of  contact  point  without  rotation  errors 

Qerr  —  rotation  error  from  vision 

T=a-  cos  (e)a  +  {b  +f)sin  (8) 

By  applying  (6.2),  (6.3)  to  the  specifics  of  Piece  1  and  Piece  2,  these  equa- 
tions become  (6.4),  (6.5). 

Xj.  -Xj  =  ?>.llsin{Q,,,)-Omisin-{^)  (6.4) 

yy'-7y  = -0.0055m  (e,,,)  +  4.365m2(^)  (6.5) 


The  translation  error  from  vision  may  not  be  larger  than  the  pseudo 
radius  of  the  stationary  puzzle  concavity,  or  the  peg  will  not  intersect  the 
concavity.  The  other  condition  requires  the  gripper  to  be  as  close  as  possible 
to  the  perpendicular  to  P\Gi,  as  discussed  in  Chapter  5.  An  estimate  for 
force  sensor  errors  due  to  lack  of  proper  positioning  requires  future  work, 
while  we  presently  address  the  geometrical  condition  only.  For  a  pseudo 
radius  of  0.25  inches,  the  condition  set  in  (6.5)  returns  a  maximum  S^^r  of  4 
degrees  for  successful  assembly.  Tests  have  been  run  with  rotation  errors 
varying  between  0  and  4.6  degrees.  Successful  nesting  was  obtained  at  initial 
rotation  errors  of  3  degrees,  while  the  test  failed  at  4.6  degrees,  results  that 
confirm  the  theoretical  prediction.  The  critical  step  of  rotation  error  elimina- 
tion had  a  success  rate  of  about  50%  mainly  due  to  lack  of  force  sensor  sen- 
sitivity. In  order  to  improve  assembly  robustness  it  may  be  necessary  to 
modify  the  nesting  algorithm  to  take  alternative  actions  in  case  edge  1  was 
not  detected.  The  algorithm  might  reverse  the  scan  on  a  perpendicular  direc- 
tion to  PiGi  to  detect  edge  3  and  4  and  then  detect  edge  1  and  2.  We  want  to 
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stress  again  that  rotational  errors  influence  both  the  geometrical  and  force 
sensing  aspects  of  assembly.  The  larger  the  rotation  error,  the  more  dif- 
ferent the  sensors  response,  which  is  due  to  sensor  asymmetry.  This  is  evi- 
dent when  comparing  Fig.  18  and  21. 
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Fig,  18  Force  sensor  output  for  puzzle  piece  "nest- 
ing" with  no  rotation  error  (tip  force) 
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Fig^  19  Force  sensor  output  for  puzzle  piece  "nest- 
ing" with  no  rotation  error  (side  force) 
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Fig.  20  Force  sensor  output  for  puzzle  piece  "nest- 
ing" with  no  rotation  error  (pitch  force) 
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Fig.   21  Force  readings  for   "nesting"   with  4.63 
degrees  rotation  error  (tip  force) 
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Fig.   22  Force  readings  for   "nesting"   with  4.63 
degrees  rotation  error  (side  force) 
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Fig.   23   Force   readings  for   "nesting"   with   4.63 
degrees  rotation  error  (pitch  force) 
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Fig.    24   Forces    for    complete    assembly    with    3 
degree  rotation  error  (tip  force) 
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Fig.  25  Force  for  complete  assembly  with  3  degree 
rotation  error  (side  force) 
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Fig.  26  Force  for  complete  assembly  with  3  degree 
rotation  error  (pitch  force) 
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analyze(analyze.c) 

#include  <stdio.h> 
#include  <vsh.h> 

analyze(true,   cgx,   cgy)      /*  analyze   the   image   and  get   eg  piece   if  "found"    */  analyze 

int  *true,   *cgx,  *cgy; 

{ 

short  buf[512]; 

int  i,  j,  imd, left, leftmost, right, rightmost; 

int  upmost,  downmost,  found; 

found=0;     /* start   with   assumption   that   no  pieces   are  present  lO 

open   image  file    *j 

imd  =   iopen("temp.  img",0);     /*   VICOM  command  use   cc   ..   —Iv   *j 

printf( "analyze   image  fileVn"); 

tryagain:  while(j<129)       /*  onlg  128  rows  in  memory   */ 

{ 

ireads(imd,buf,512);  /  *read  one  line  at  a  time   */ 

/  *pnntf("row   %d  read\n",  j);    */ 

for(i=30;i<483&fcbuf[i]==0.0;i=i+4); 

/*prtntf("  1=   %d\n".    i);    */ 

if  ((i<483).5:&(buf(i]<0.0)) 

{ 
/*  big  loop   to   determine   if  object   was  found    */ 

leftmost=i; 
upmost=j; 
/*prtntf("leftmost=  %(f\n",   leftmost);     */ 

I  *printf("upmost=   %d\n",    upmost);      */ 
for(;i<483&i:buf[i]<0.0;i++) 

{ 
/  *scan   the   line   to  see   where  the   object   ends    */ 

rightmost=i; 

} 

/  *printf("rightmost=%d\n" ,    rightmost);   */ 

newline:  j=j  +  l; 

if(j<130) 

{ 

/*  check  to  see  if  the   bottom  of  picture  was  reached   */ 

ireads(imd,buf,5r2); 

/*printff"line  %d  read\n",  j);   */  40 

/*  read  new  line   after  first  that   was  not   empty    */ 

for(i=30;i<483A;&buf[i]==0.0;i=i+4); 

if((i<483)i:i:(buf[i]<0.0)) 

{ 

left=i; 

if(leftmost>left)  leftmost=left; 

/  *printf("newleftmost=%d\n" , leftmost); 

7 

/*  readjust  left   margin   */ 

for(;i<483A:&buf[i]<0.0;i++)       '  "  so 

{ 

right=i; 

} 
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analyze(analyze.c) 


/*   readjust   right   margin    */ 


I  *read  in   newline   only   if  not  junk    */ 


if( right > rightmost)   rightmost =right; 
/  *printf("newrighimosi—%d\n", 
rightmost);*/ 

downmost=j; 

/  *printf("downmost=%(f\n",    downmost):*/ 

found=l; 
goto  newline; 


60 


else 

{ 
/*  empty  line  found   affer  full  line(s)    */ 


lf(!found) 


j=j+i; 

goto  tryagain; 

'  ■ '  \' 

/*  discard  results  and  start   all  over  again   ,    it    was  a  junk  point    */ 

■   "I> 

} 

^.  .^^'^ 

} 

}      /*  end   of  for  j<  130  loop    */ 

/*  here   when 

object  found  is   not  junk   *j 

if(found)  goto  calculate; 
}   /*  end  of  large   if  loop    */ 

}     /*  end  of  big   while  loop    */ 

calculate: 

if((j>128)A;&(!found)) 

{ 

*true=0; 
*cgx=0; 
*cgy=0; 
return; 

} 
else 

{ 

70 


if(found) 
{ 


} 


*true=l; 

*cgx=(Ieftmost  +  rightmost)/2; 

*cgy=3*(upmost+dovvnmost  — '2)/2  +  48; 


iclose(imd); 

}   /*  end  of  else    */ 
}   /*  end   of  analyze    */ 
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approach(approach.c) 


Inputs: 


p2_i,y: 

lefi_x: 
theta: 
jl_i,y: 


Approach. c  routine   calculates  the  firsCmove   after  robot 
has  picted  up  piece   2.     In   this  calculations,    data   is 
provided  by  vision   routines  and   match   routine. 

center  of  gravity   of  moving  piece   (piece2) 

center  of  gravity   of  matching  side   of  moving  piece 

center  of  gravity  of  matching  side   of  stationary  piece 

(piecel) 

leftmost  I  on   image   of  stationary  piece 

rotation   angle   of  matching  side 

robot  x,y  arm   coordinates   when  stationary  piece   is 

"centered"   in   the   camera  field  of  view. 
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Outputs: 


]3_x.  y: 


robot  arm  x.y   coordinates  for  approach  prior  to 
assembly 


#include  <stdio.h> 
#include  <math.h> 
#include  "transform. h" 


approach(gl_x,glj',pl_x,pl_y,p2_x,p2_y,left_x,thetajl_xjl_yj3_xj3_>') 


approach 


int     gl_x,gl_y,pl_x,pl_y,p2_x,p2_y,left_x; 
float  theta jl_x,  jl_y,  *j3_x,*j3_y; 

{ 

float     j3_xtemp,  j3_ytemp; 
int         g3_x,  g3_y; 
/*  g3_x,y:     center  of  gravity  of  moving  piece  for  end  of  approaching  move   as 
shown  on   vicom  image 

7 

theta=theta*3. 1415927/180.; 

I  *transform   theta   in   radians     to   be   used  by  trigonometric  library   */ 


30 


g3.x=   (p2_x  +   left_x*(XSCALE-l)   +  0.5)/XSCALE  +  sin(theta)*(gl_y   -   ^\j)  + 

cos(theta)*(gl_x  -   pl_x)/XSCALE; 
g3_y=  p2_v  +  sin(thela)*(pl_x  -  gl_x)/XSCALE  +  cos(theta)*(gl_y   -   pl_y); 
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imgvsrobt(g3_x,g3_y,  &:j3_xtemp,  cSc;j3_ytemp) 

*j3_x=  j3_xtemp  +  jl_x; 
*J3j'=  j3_vaemp   +  jl_y; 


priiitf(  "robot  x   target  for  approach  =  '/,f\n",   *j3_x); 
printf( "robot  y   taxget  for  approach  =  '/.f\n",   *j3_y); 
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} 
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